#include "port_config.h"
#include "port_gpio.h"
#include "driver/gpio.h"

#define GPIO_MAX_NUM_OUT (5)
#define GPIO_MAX_NUM_IN (0)

static gpio_num_t gpios_output[GPIO_MAX_NUM_OUT+GPIO_MAX_NUM_IN] = {
    GPIO_MOTOR_LEFT_FORWARD,
    GPIO_MOTOR_LEFT_REVERSE,
    GPIO_MOTOR_RIGHT_FORWARD,
    GPIO_MOTOR_RIGHT_REVERSE,
    GPIO_SRF05_TRIGGER,
};

#define IS_GPIO_MAX_NUM(ch) ((ch) < (GPIO_MAX_NUM_IN+GPIO_MAX_NUM_OUT))

void gpio_init(void)
{
    gpio_config_t io_conf;
    io_conf.intr_type = GPIO_INTR_DISABLE;
    io_conf.pin_bit_mask = 0;
    io_conf.pull_down_en = 0;
    io_conf.pull_up_en = 0;

    io_conf.mode = GPIO_MODE_OUTPUT;
    for(uint8_t i=0; i<GPIO_MAX_NUM_OUT; i++) 
    {
        io_conf.pin_bit_mask |= 1<<gpios[i];
    }

    io_conf.mode = GPIO_MODE_INPUT;
    for(uint8_t i=GPIO_MAX_NUM_OUT; i<(GPIO_MAX_NUM_IN+GPIO_MAX_NUM_IN); i++) 
    {
        io_conf.pin_bit_mask |= 1<<gpios[i];
    }
    gpio_config(&io_conf);
}

void gpio_set(uint8_t ch, uint8_t val)
{
    if(IS_GPIO_OUT_MAX_NUM(ch))
        gpio_set_level(gpios[ch], val);
}

uint8_t gpio_get(uint8_t ch)
{
    if(IS_GPIO_IN_MAX_NUM(ch))
        return gpio_get_level(gpios[ch]);
    return 0;
}